#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h" 
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include<sstream>
#include <iostream>
#include <string>

#include <cstdio>
#include <cstdlib>
#include <string.h>

#include <fstream>
#include <iostream>
#include <iomanip>
using namespace std;
using namespace std;
std::string Convert(float Num)
{
    std::ostringstream oss;
    oss<<Num;
    std::string str(oss.str());
    return str;
}
std::string Ltos(long l)  
{  
    ostringstream os;  
    os<<l;  
    string result;  
    istringstream is(os.str());  
    is>>result;  
    return result;  
  
} 
std::string fname;
uint state;
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
    // ROS_INFO("I heard the pose from the robot"); 
    // ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    // ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    // ROS_INFO("the time we get the pose is %f",  msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);
    // ROS_INFO("the time we get the pose is %f",  msg->header.stamp);
    float pose_x=0.0,pose_y=0.0,pose_z=0.0,ori_x=0.0,ori_y=0.0,ori_z=0.0,ori_w=0.0;

    pose_x = msg->pose.position.x;
    pose_y = msg->pose.position.y;
    pose_z = msg->pose.position.z;
    ori_x  = msg->pose.orientation.x;
    ori_y  = msg->pose.orientation.y;
    ori_z  = msg->pose.orientation.z;
    ori_w  = msg->pose.orientation.w;

    // std::cout<<sp<<std::endl;

    std::string str_px =  Convert(pose_x );
    std::string str_py =  Convert(pose_y );
    std::string str_pz =  Convert(pose_z );

    std::string str_ox =  Convert(ori_x);
    std::string str_oy =  Convert(ori_y);
    std::string str_oz =  Convert(ori_z);
    std::string str_ow =  Convert(ori_w);

    std::string str_tmI =  Ltos(msg->header.stamp.sec);
    std::string str_tmN =  Ltos(msg->header.stamp.nsec);

    std::string dat_str = str_tmI + "," + str_tmN + "," + str_px + "," + str_py + "," + str_pz + "," + str_ox + "," + str_oy + "," + str_oz + "," + str_ow ;
    // std::ofstream output_file("./testfile.dat");
    // std::ofstream  out1("./point.txt",std::ios::app);
    if(state == 0)
    {
        return;
    }else if(state == 1){
        fname = str_tmI + +".txt";
        state = 2;
        return;
    }
    std::ofstream out(fname,std::ios::app);
    out<<dat_str<<std::endl;
    std::cout<<dat_str<<std::endl;
    out.close();
    // std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
}
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    ROS_INFO("I heard the pose from the robot");
    if(msg->armed)
    {
        state = 1;
    }else
        state = 0;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "record");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("mavros/vision_pose/pose", 1000, poseCallback);
  ros::Subscriber state_sub = n.subscribe("mavros/state", 10, state_cb);
  ros::spin();

  return 0;
}